#include <cassert>
#include <boost/algorithm/string.hpp>
#include <map>

#include "../../../XMLTools.h"

#include "WholeBodyDynamicSensor.h"

using namespace MMM;

WholeBodyDynamicSensorPtr WholeBodyDynamicSensor::loadSensorXML(RapidXMLReaderNodePtr node) {
    WholeBodyDynamicSensorPtr sensor(new WholeBodyDynamicSensor());
    sensor->loadSensor(node);
    return sensor;
}

WholeBodyDynamicSensor::WholeBodyDynamicSensor(const std::string &description) : InterpolatableSensor(description)
{
}

bool WholeBodyDynamicSensor::checkModel(ModelPtr model) {
    return true;
}

boost::shared_ptr<BasicSensor<WholeBodyDynamicSensorMeasurement> > WholeBodyDynamicSensor::cloneConfiguration() {
    WholeBodyDynamicSensorPtr m(new WholeBodyDynamicSensor(description));
    return m;
}

void WholeBodyDynamicSensor::loadConfigurationXML(RapidXMLReaderNodePtr node) {
    assert(node);
    assert(node->name() == "Configuration");
}

void WholeBodyDynamicSensor::loadMeasurementXML(RapidXMLReaderNodePtr node, float timestep, SensorMeasurementType type)
{
    assert(node);
    assert(node->name() == XML::NODE_MEASUREMENT);

    RapidXMLReaderNodePtr comNode = node->first_node("CenterOfMass");
    RapidXMLReaderNodePtr amNode = node->first_node("AngularMomentum");
    WholeBodyDynamicSensorMeasurementPtr m(new WholeBodyDynamicSensorMeasurement(timestep, XML::readFloatArray(comNode->value(), 3), XML::readFloatArray(amNode->value(), 3), type));
    addSensorMeasurement(m);
}

void WholeBodyDynamicSensor::appendConfigurationXML(RapidXMLWriterNodePtr node)
{
    assert(node);

    node->append_node("Configuration");
}

bool WholeBodyDynamicSensor::hasSensorConfigurationContent() const
{
    return true; // TODO
}

bool WholeBodyDynamicSensor::equalsConfiguration(SensorPtr other) {
    WholeBodyDynamicSensorPtr ptr = boost::dynamic_pointer_cast<WholeBodyDynamicSensor>(other);
    if (ptr) {
        return true;
    }
    else return false;
}

std::string WholeBodyDynamicSensor::getType() {
    return TYPE;
}

std::string WholeBodyDynamicSensor::getVersion() {
    return VERSION;
}

int WholeBodyDynamicSensor::getPriority() {
    return 80;
}

